#ifndef __D_STAR__
#define __D_STAR__

#include "meta_search.hpp"
#include <iostream>
#include <vector>
#include <queue>
#include <cmath>

namespace graph_search{
    struct N{
        int x,y;
        double g,rhs;
        N(int x,int y,double g = 0,double rhs = 0):x(x),y(y),g(g),rhs(rhs){}
    };

    class D_star:public search{
    private:
        bool isValid(int x,int y,int n){
            return x>=0 && y>=0 && x<n && y<n;
        }
        int heuristic(int x1,int y1,int x2,int y2){
            return abs(x1-x2) + abs(y1-y2);
        }
        void update_vertex(const N& s,const N& goal,
                    const std::vector<std::vector<int>>& grid,
                    std::vector<std::vector<N>>& nodes,
                    std::priority_queue<N>& openSet){
            if(s.x ==goal.x && s.y == goal.x){
                return;
            }
            double minValue = std::numeric_limits<double>::max();
            for(int i=0; i<4;++i){
                int x = s.x + dx[i];
                int y = s.y + dy[i];
                if(isValid(x,y,grid.size()) && grid[x][y] == 0){
                    minValue = min(minValue,nodes[x][y].g + heuristic(nodes[x][y].x,nodes[x][y].y,s.x,s.y));
                }
            }
            nodes[s.x][s.y].rhs = minValue;
            N nodeInQueue = N(s.x,s.y,nodes[s.x][s.y].rhs,nodes[s.x][s.y].g);
            if(nodes[s.x][s.y].rhs != nodes[s.x][s.y].g){
                openSet.push(nodeInQueue);
            }
        }
    public:
        int find(const std::vector<std::vector<int>>& matrix,Position start,Position end){
            N startPoint(start.x,start.y);
            N endPoint(end.x,end.y);
            int n = matrix.size();
            std::vector<std::vector<N>> nodes(n, std::vector<N>(n));
            std::priority_queue<N> openSet;

            for(int x=0;x<n;++x){
                for(int y=0;y<n;++y){
                    nodes[x][y] = N(x,y,std::numeric_limits<double>::max(),std::numeric_limits<double>::max());
                }
            }

            nodes[endPoint.x][endPoint.y].rhs = 0;
            openSet.push(N(endPoint.x,endPoint.y,std::numeric_limits<double>::max(),0));

            while(!openSet.empty()){
                N current = openSet.top();
                openSet.pop();

                if(current.x==startPoint.x && current.y == startPoint.y){
                    return 1;
                }

                if(nodes[current.x][current.y].g != current.g || nodes[current.x][current.y].rhs != current.rhs){
                    continue;
                }

                nodes[current.x][current.y].g = nodes[current.x][current.y].rhs;

                for(int i=0;i<4;++i){
                    int x = current.x + dx[i];
                    int y = current.y + dy[i];

                    if(isValid(x,y,n) && matrix[x][y] == 0){
                        update_vertex(nodes[x][y],endPoint,matrix,nodes,openSet);
                    }
                }
            }
        }
    };
}

#endif